#pragma config(Sensor, in1,    light,          sensorReflection)
#pragma config(Sensor, in2,    line1,          sensorLineFollower)
#pragma config(Sensor, in3,    line2,          sensorLineFollower)
#pragma config(Sensor, in4,    line3,          sensorLineFollower)
#pragma config(Sensor, dgtl1,  bumper,         sensorTouch)
#pragma config(Sensor, dgtl2,  sonar,          sensorSONAR_inch)
#pragma config(Sensor, dgtl4,  bumper2,        sensorTouch)
#pragma config(Motor,  port2,           LeftBack,      tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port3,           LeftFront,     tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           RightBack,     tmotorVex269, openLoop)
#pragma config(Motor,  port5,           RightFront,    tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define stopped 0
#define mforward 1
#define mbackward 2
#define tleft 3
#define tright 4
#define abort 5
#define arcleft 6
#define arcright 7
#define arcarc 8
void MoveForward(int speed);
int GetLight();
bool IsLight(int threshold = 130);
void StopMove();
void TurnLeft(int turntime);
bool IsBumper();
bool IsBumper2();
void TurnRight(int turntime);
void Move();
int GetLine1();
int GetLine2();
int GetLine3();
void IncreaseSpeed(int dir, int amount,int speed);
void MoveWithLight();
bool IsLineR(int threshold = 600);
bool IsLineM(int threshold = 600);
bool IsLineL(int threshold = 600);
void CheckStuff();
bool IsSonar(int threshold = 8);
int GetSonar();
void ReduceSpeed(int dir, int amount,int speed);
void ArcLeft(int speed = 40);
void boxarcleft (int speed = 40);
task main()
{
  GetSonar();
  bool on = true;
  int speed;
  StopMove();
  int state = stopped;
  while (on == true)
  {
    CheckStuff();
    switch (state)
    {
    case stopped:
      if (IsLight() == true)
      {
        speed = 40;
        MoveForward(speed);
        state = mforward;
        break;
      }
      break;
    case mforward:
      if (IsSonar() == true)
      {
        StopMove();
        boxarcleft();
        state = arcleft;
        break;
      }
      if (IsLineR() == true)
      {
        state = tright;
        break;
      }
      else if (IsLineL() == true)
      {
        state = tleft;
        break;
      }
      else if (IsLineM() == true)
      {
        MoveForward(speed);
        state = mforward;
        break;
      }
      else if (IsLineL() && IsLineM() && IsLineR() == false)
      {
        state = abort;
        break;
      }
      break;
    case mbackward:
      speed = -80;
      MoveForward(speed);
      break;
    case tleft:
      ReduceSpeed(0,25,speed);
      IncreaseSpeed(1,25,speed);
      if (IsLineR() == true)
      {
        state = tright;
        break;
      }
      else if (IsLineL() == true)
      {
        state = tleft;
        break;
      }
      else if (IsLineM() == true)
      {
        MoveForward(speed);
        state = mforward;
        break;
      }
      break;
    case tright:
      ReduceSpeed(1,25,speed); //0 is left
      IncreaseSpeed(0,25,speed);//1 is right
      if (IsLineR() == true)
      {
        state = tright;
        break;
      }
      else if (IsLineL() == true)
      {
        state = tleft;
        break;
      }
      else if (IsLineM() == true)
      {
        MoveForward(speed);
        state = mforward;
      }
      break;
    case arcleft:
      if (IsSonar == true)
      {
        state = abort;
        break;
      }
      else if (IsLineM == true)
      {
        state = mforward;
        break;
      }
      else
      {
        state = abort;
        break
      }
    case abort:
      on = false;
      break;
    }
  }
}
int GetSonar()
{
  int sonard = SensorValue[sonar];
  return sonard;
}
bool IsSonar(int threshold)
{
  int sonard = GetSonar();
  if ((sonard <= threshold) && (sonard >= 0))
  {
    return true;
  }
  else if (sonard > threshold)
  {
    return false;
  }
  else
  {
    return false;
  }
}
void IncreaseSpeed(int dir, int amount,int speed)
{
  if (dir == 1)
  {

    motor[RightFront] = speed + amount;
    motor[RightBack] = speed + amount;
  }
  else if (dir == 0)
  {
    motor[LeftFront] = speed + amount;
    motor[LeftBack] = speed + amount;
  }
}
void CheckStuff()
{
  IsBumper();
  IsBumper2();
  IsLight();
  IsLineR();
  IsLineM();
  IsLineL();
  GetSonar();
}
void ReduceSpeed(int dir, int amount,int speed)
{
  if (dir == 1)
  {
    motor[RightFront] = speed - amount;
    motor[RightBack] = speed - amount;
  }
  else if (dir == 0)
  {
    motor[LeftFront] = speed - amount;
    motor[LeftBack] = speed - amount;
  }
}
void MoveForward(int speed)
{
  motor[LeftFront] = speed;
  motor[LeftBack] = speed;
  motor[RightFront] = speed;
  motor[RightBack] = speed;
}
void MoveWithLight()
{
  bool on=true;
  while (on == true)
  {
    while (GetLight() <= 120)
    {
      Move();
    }
    StopMove();
  }
}
void ArcLeft(int speed)
{
  IncreaseSpeed(0,25,speed);
  if (IsLineM == true)
  {
    StopMove();
    TurnLeft(70);
  }
}
void boxarcleft (int speed)
{
  TurnLeft(70);
  MoveForward(speed);
  wait10Msec(80);
  StopMove();
  TurnRight(75);
  MoveForward(speed);
  wait10Msec(80);
  TurnRight(75);
  MoveForward(speed);
  wait10Msec(80);
  StopMove();
  TurnLeft(75);
}
void TurnLeft(int turntime)
{
  motor[LeftFront] = -60;
  motor[LeftBack] = -60;
  motor[RightFront] = 60;
  motor[RightBack] = 60;
  wait10Msec(turntime);
}
void TurnRight(int turntime)
{
  motor[LeftFront] = 60;
  motor[LeftBack] = 60;
  motor[RightFront] = -60;
  motor[RightBack] = -60;
  wait10Msec(turntime);
}
void Move()
{
  if (IsBumper() == true)
  {
    TurnLeft(75);
  }
  else if (IsBumper2() == true)
  {
    TurnRight(75);
  }
  else
  {
    int speed = 80;
    MoveForward(speed);
  }
}
void StopMove()
{
  motor[LeftFront] = 0;
  motor[LeftBack] = 0;
  motor[RightFront] = 0;
  motor[RightBack] = 0;
}
int GetLight()
{
  int lightsensor = SensorValue[light];
  return lightsensor;
}
bool IsLight(int threshold)
{
  int lightsensor = GetLight();
  if (lightsensor <= threshold)
  {
    return true;
  }
  else if (lightsensor > threshold)
  {
    return false;
  }
  else
  {
    return false;
  }
}
bool IsBumper()
{
  bool bumpervalue;
  if (SensorValue[bumper] == 1)
  {
    bumpervalue = true;
  }
  else
  {
    bumpervalue = false;
  }
  return bumpervalue;
}
bool IsBumper2()
{
  bool bumpervalue;
  if (SensorValue[bumper2] == 1)
  {
    bumpervalue = true;
  }
  else
  {
    bumpervalue = false;
  }
  return bumpervalue;
}
int GetLine1()
{
  int linesr = SensorValue[line1];
  return linesr;
}
int GetLine2()
{
  int linesm = SensorValue[line2];
  return linesm;
}
int GetLine3()
{
  int linesl = SensorValue[line3];
  return linesl;
}
bool IsLineR(int threshold)
{
  int temp = GetLine1();
  if (temp <= threshold)
  {
    return true;
  }
  else if (GetLine1() > threshold)
  {
    return false;
  }
  else
  {
    return false;
  }
}
bool IsLineM(int threshold)
{
  if (GetLine2() <= threshold)
  {
    return true;
  }
  else if (GetLine2() > threshold)
  {
    return false;
  }
  else
  {
    return false;
  }
}
bool IsLineL(int threshold)
{
  if (GetLine3() <= threshold)
  {
    return true;
  }
  else if (GetLine3() > threshold)
  {
    return false;
  }
  else
  {
    return false;
  }
}
